clc;
clear;
close all;


% 1、验证DH参数 passed
alpha = [pi/2, -pi/2, pi/2, pi/2, pi/2];
a = [0, 0, 0.01491, 0, 0];

q = [pi/2, -1.5359, 0, pi, pi/2];
d = [0, 0, 0.27898, 0, 0.1085];

for i=1:5
    T(:,:,i)=[
            cos(q(i)), -sin(q(i)), 0, a(i);
            sin(q(i)).*cos(alpha(i)), cos(q(i))*cos(alpha(i)), -sin(alpha(i)), -d(i)*sin(alpha(i));
            sin(q(i))*sin(alpha(i)), cos(q(i))*sin(alpha(i)), cos(alpha(i)), d(i)*cos(alpha(i));
            0,0,0,1];
end

% 在axis_0坐标系下某点坐标
P = [0.05896; 0.139; -0.28990; 1];
T01=T(:,:,1); P_a1 = inv(T01)*P; % 计算该点在axis_1坐标系下的值
T02=T01*T(:,:,2); P_a2 = inv(T02)*P; % 计算该点在axis_2坐标系下的值
T03=T02*T(:,:,3); P_a3 = inv(T03)*P; % 计算该点在axis_3坐标系下的值
T04=T03*T(:,:,4); P_a4 = inv(T04)*P; % 计算该点在axis_4坐标系下的值
T05=T04*T(:,:,5); P_a5 = inv(T05)*P; % 计算该点在axis_4坐标系下的值


